#include "handDet.h"
/***********************************************************************
	Function:				handdetction
	Description:			Using the haar feature to detect the hand .
	Input:					img						--		the three-channel BGR image
								cascade				--		if ROIexist is false,the routine will use the cascade
																	to search the whole img
								nestedCascade	--		if ROIexist if true,the routine will use the nestedCascade
																	to search the ROI of the img
								scale					--		image scale
								ROIexist				--     if ROIexist is false,the routine will use the cascade
																	to search the whole img
																	if ROIexist if true,the routine will use the nestedCascade
																	to search the ROI of the img
								ROI						--		the routine will detect the hand in the ROI of the img
	Output:				void
	Author:					XiaoBin,modified by GuoKailing and XiaoBin
	Data:					2010.5.20
***********************************************************************/
void handDetction( Mat& img,
								CascadeClassifier& cascade,
								CascadeClassifier& nestedCascade,
								double scale,
								bool & ROIexist,
								Rect&  ROI)
{

	int i = 0;
	double t = 0;
	vector<Rect> hands,hands1;
	Point LeftTop,RightBottom;
	const static Scalar colors[] =  { CV_RGB(0,0,255),
		CV_RGB(0,128,255),
		CV_RGB(0,255,255),
		CV_RGB(0,255,0),
		CV_RGB(255,128,0),
		CV_RGB(255,255,0),
		CV_RGB(255,0,0),
		CV_RGB(255,0,255)} ;
	Mat gray, smallImg( cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1 );
	cvtColor( img, gray, CV_BGR2GRAY );
	resize( gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR );
	equalizeHist( smallImg, smallImg );

	if (ROIexist==0)
	{
		t = (double)cvGetTickCount();
		cascade.detectMultiScale( smallImg, hands,0.2,1.1, 3, 0	,cv::Size(20, 20) );
		t = (double)cvGetTickCount() - t;
		/*printf( "detection time1 = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );*/
		if (!hands.empty())
		{
			for( vector<Rect>::const_iterator r = hands.begin(); r != hands.end(); r++, i++ )
			{

				Scalar color = colors[i%8];
				LeftTop.x = cvRound(r->x * scale);
				LeftTop.y = cvRound(r->y * scale);
				RightBottom.x = cvRound((r->x + r->width)*scale);
				RightBottom.y = cvRound((r->y + r->height)*scale);
				rectangle(img,LeftTop,RightBottom,color,3,8,0);
			}

			ROI = hands[0];

			ROI.x=cvRound(ROI.x-0.5*ROI.width);
			ROI.y=cvRound(ROI.y-0.5*ROI.height);
			if (ROI.x<0)
			{
				ROI.x = 0;
			}
			if (ROI.y<0)
			{
				ROI.y = 0;
			}
			ROI.width=2*ROI.width;
			ROI.height=2*ROI.height;
			if (ROI.width+ROI.x>smallImg.cols)
			{
				ROI.width = max(smallImg.cols-ROI.x,0);
			}
			if (ROI.height+ROI.y>smallImg.rows)
			{
				ROI.height = max(smallImg.rows-ROI.y,0);
			}
			LeftTop.x = cvRound((ROI.x)* scale);
			LeftTop.y = cvRound((ROI.y)* scale);
			RightBottom.x = cvRound((ROI.x + ROI.width)*scale);
			RightBottom.y = cvRound((ROI.y + ROI.height)*scale);
			rectangle(img,LeftTop,RightBottom,colors[6],3,8,0);
			ROIexist=1;
		}
	}
	else
	{
		Mat smallImgROI = smallImg(ROI);
		t = (double)cvGetTickCount();
		nestedCascade.detectMultiScale( smallImgROI, hands,0.2,1.1, 3, 0	,Size(20, 20) );
		//t = (double)cvGetTickCount() - t;
		/*printf( "detection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );*/
		if (!hands.empty())
		{
			for( vector<Rect>::const_iterator d = hands.begin(); d != hands.end(); d++, i++ )
			{
				Scalar color = colors[i%8];
				LeftTop.x = cvRound((d->x +ROI.x) * scale);
				LeftTop.y = cvRound((d->y +ROI.y) * scale);
				RightBottom.x = cvRound((d->x + d->width +ROI.x)*scale);
				RightBottom.y = cvRound((d->y + d->height +ROI.y)*scale);
				rectangle(img,LeftTop,RightBottom,color,3,8,0);
			}
			LeftTop.x = cvRound((ROI.x)* scale);
			LeftTop.y = cvRound((ROI.y)* scale);
			RightBottom.x = cvRound((ROI.x + ROI.width)*scale);
			RightBottom.y = cvRound((ROI.y + ROI.height)*scale);
			rectangle(img,LeftTop,RightBottom,colors[6],3,8,0);

			ROI.x=cvRound(ROI.x+hands[0].x-0.5*hands[0].width);
			ROI.y=cvRound(ROI.y+hands[0].y-0.5*hands[0].height);
			if (ROI.x<0)
			{
				ROI.x = 0;
			}
			if (ROI.y<0)
			{
				ROI.y = 0;
			}
			ROI.width=2*hands[0].width;
			ROI.height=2*hands[0].height;
			if (ROI.width+ROI.x>smallImg.cols)
			{
				ROI.width = max(smallImg.cols-ROI.x,0);
			}
			if (ROI.height+ROI.y>smallImg.rows)
			{
				ROI.height = max(smallImg.rows-ROI.y,0);
			}
		}
		else
		{
			double angle[]={45,30,15,-45,-15,-30};

			for (int j=0;j<6;j++)
			{

				Mat rot_mat(2,3,CV_32FC1);
				Mat wrot_mat(2,3,CV_32FC1);

				Point center( cvRound(ROI.x+ROI.width/2), cvRound(ROI.y+ROI.height/2) );
				rot_mat = getRotationMatrix2D( center, angle[j], 1 );

				wrot_mat = getRotationMatrix2D( center, -angle[j],1 );

				Mat smallImgROI_rota;
				warpAffine(smallImg,smallImgROI_rota,rot_mat,smallImg.size());

				Mat rotaed_roi = smallImgROI_rota(ROI);
				t = (double)cvGetTickCount();
				nestedCascade.detectMultiScale( rotaed_roi, hands,0.2,1.2, 3, 0	,Size(20, 20) );
				t = (double)cvGetTickCount() - t;
				//printf( "rotatedetection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );
				if (!hands.empty())
				{
					for( vector<Rect>::const_iterator d = hands.begin(); d != hands.end(); d++, i++ )
					{
						Scalar color = colors[i%8];
						LeftTop.x = cvRound((d->x +ROI.x) * scale);
						LeftTop.y = cvRound((d->y +ROI.y) * scale);
						RightBottom.x = cvRound((d->x + d->width +ROI.x)*scale);
						RightBottom.y = cvRound((d->y + d->height +ROI.y)*scale);
                        Mat  tmpImg2;
                        Mat tmpImg1 = Mat::zeros(img.size(), img.type());
                        //warpAffine(tmpImg1, tmpImg1, rot_mat, tmpImg1.size());
						rectangle(tmpImg1,LeftTop,RightBottom,CV_RGB(0,0,255),3,8,0);
                        warpAffine(tmpImg1, tmpImg2, wrot_mat, tmpImg1.size());
                        //imshow("tmp",tmpImg1);
                        add(img, tmpImg2, img);
					}
					LeftTop.x = cvRound((ROI.x)* scale);
                    LeftTop.y = cvRound((ROI.y)* scale);
                    RightBottom.x = cvRound((ROI.x + ROI.width)*scale);
                    RightBottom.y = cvRound((ROI.y + ROI.height)*scale);
                    rectangle(img,LeftTop,RightBottom,colors[6],3,8,0);
				break;
			}
		}
		if (hands.empty())
			ROIexist=0;
		}
	}
	//char temp[54];
	//sprintf(temp,"detection time = %g ms\0", t/((double)cvGetTickFrequency()*1000.));
	//cv::putText(img,temp,cvPoint(50,50),FONT_HERSHEY_SIMPLEX,0.5,RGB(123,46,32));
	//cv::imshow( "result", img );
}
